import cv2
from picamera2 import Picamera2
import queue
import logging
import threading
from datetime import datetime

Picamera2.set_logging(Picamera2.WARNING)

class Camera:
    def __init__(self):
        self.run = False
        self.__log_init()

    def __log_init(self):
        self.logger = logging.getLogger("Camera")

    def open(self):
        self.logger.trace('open.enter')
        self.picam2 = Picamera2()
        config = self.picam2.create_video_configuration(
            main={"size": (640, 360), "format": "BGR888"},
            controls={"FrameRate": 30}
        )
        self.picam2.configure(config)
        self.picam2.start()
        self.logger.trace('open.done')
        return True

    def close(self):
        self.logger.trace('close.enter')
        self.picam2.stop()
        self.logger.trace('close.done')


    def thread(self):
        self.logger.trace('thread.enter')
        number = 0

        while self.run:
            frame = self.picam2.capture_array()
            # frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)

            # number is used to identify the frame
            # aim is to facilitate traceging
            number += 1

            # 给"数字识别"任务传递图像
            try:
                if self.outqueue1.full():
                    self.outqueue1.get_nowait()
                self.outqueue1.put([frame, number], timeout=0.1)
                # self.logger.spam("rgb put: %d", number)
            
            except queue.Full as e:
                self.logger.warning('output rgb full!')
            except Exception as e:
                self.logger.error('output rgb error!')
                self.logger.error('output rgb exception: %s', e)

            frame_480 = cv2.resize(frame, (640, 480), frame)
            # 给"路口标志检测"任务传递图像
            try:
                if self.outqueue2.full():
                    self.outqueue2.get_nowait()
                self.outqueue2.put([frame_480, number], timeout=0.1)
                # self.logger.spam("rgb put: %d", number)
            except queue.Full as e:
                self.logger.warning('output rgb full!')
            except Exception as e:
                self.logger.error('output rgb error!')
                self.logger.error('output rgb exception: %s', e)

            # 给"巡线"任务传递图像
            try:
                if self.outqueue3.full():
                    self.outqueue3.get_nowait()
                self.outqueue3.put([frame_480, number], timeout=0.1)
                # self.logger.spam("rgb put: %d", number)
            except queue.Full as e:
                self.logger.warning('output rgb full!')
            except Exception as e:
                self.logger.error('output rgb error!')
                self.logger.error('output rgb exception: %s', e)
            
            # self.logger.error("rgb put: %d", number)

        self.logger.trace('thread.done')

    def start(self):
        self.logger.trace('start.enter')

        self.run = True
        self.handle = threading.Thread(target=self.thread)
        self.handle.start()

        self.logger.trace('start.done')
        return True

    def stop(self):
        self.logger.trace('stop.enter')
        self.run = False
        self.handle.join()
        self.logger.trace('stop.done')

    def output_queue_set(self, output_queue_list):
        self.logger.trace('output_queue_set.enter')

        if self.run:
            self.logger.error("camera is opened, please close the camera first!")
            return False
        
        for output_queue in output_queue_list:
            if not isinstance(output_queue, queue.Queue):
                self.logger.error("one queue is not a queue!")
                return False

        # 本项目中，需要摄像头输出两个一样的rgb队列
        self.outqueue1 = output_queue_list[0]   # 传递给“数字识别”任务的图像
        self.outqueue2 = output_queue_list[1]   # 传递给“路口标志检测”任务的图像
        self.outqueue3 = output_queue_list[2]   # 传递给“巡线”任务的图像

        self.logger.trace('output_queue_set.done')
        return True